#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <math.h>
#include <iostream>
#include <iomanip>
#include <limits>

#include <omp.h>

#include "config.h"
#include "fish.h"
#include "quadfish.h"
#include "QuadTree.h"

using namespace std;

double v0x, v0y, v1x, v1y, v2x, v2y, v3x, v3y, v4x, v4y, v5x, v5y, vx, vy, ax, ay; // vectors
double cogx, cogy; // center of gravity coordinates
double local_cogx, local_cogy;

FILE *output_fp; /* Pointer to output file */
FILE *input_fp; /* Pointer to input file */
bool outputp;
bool inputp;

int n_fish = 10; /* Other defaults in fish-lib.c. */
fish_t *fish; /* Global array of fish */

struct Timer total_timer;

int numSplits = 0;
int numCollapses = 0;

void init_fish();
void output_fish( FILE* output_fp, const double t, const double dt,
                  const fish_t* fish, const int n_fish );
void doStuff();

int numThreads;

int main( int argc, char **argv )
{
    assert( R_SIGHT2 > R_CROWD2 );
    //    min_r2 = numeric_limits<double>::epsilon();
    //    min_r = sqrt( min_r2 );

    get_options( argc, argv );
//    srand48( clock() );
    srand48( 0 );
    cout << "numThreads\tnumfish\t\tCPUtime\t\tsimtime\tsteps" << endl;
//    cout << "MinFish\tnumCollapses\tMaxFish\tnumSplits\tCPUtime\t\tsimtime\tsteps" << endl;

//    for averaging
    for ( int i = 0; i < 10; ++i ) {
        srand48( 0 );
        numSplits = 0;
        numCollapses = 0;
        doStuff();
    }

    //for varying min/maxfish
//    QuadTree::MaxFish = 9;
//    for ( int minf = 1;
//            minf <= QuadTree::MaxFish;
//            minf += 1 ) {
//        QuadTree::MinFish = minf;
//        numSplits = 0;
//        numCollapses = 0;
//        srand48( 0 );
//        doStuff();
//    }
//    for ( int maxf = 2;
//            maxf <= 40;
//            maxf += 1 ) {
//        QuadTree::MaxFish = maxf;
//        QuadTree::MinFish = .7 * maxf;
//        numSplits = 0;
//        numCollapses = 0;
//        srand48( 0 );
//        doStuff();
//    }

    return 0;
}

void doStuff()
{
    double curr_time;
    double dt = 0.0;
    int steps;
    double output_time;
    double max_norm;
    int myID, moveTo;
    FishThread *threads;
    int count;

    if ( output_filename ) {
        outputp = true;
        output_fp = fopen( output_filename, "w" );
        if ( output_fp == NULL ) {
            printf( "Could not open %s for output\n", output_filename );
            exit( 1 );
        }
        fprintf( output_fp, "n_fish: %d\n", n_fish );
    }

    if ( input_filename ) {
        inputp = true;
        input_fp = fopen( input_filename, "r" );
        if ( input_fp == NULL ) {
            printf( "Could not open %s for input\n", input_filename );
            exit( 1 );
        }
    }

    init_fish();

    if ( inputp ) {
        fclose( input_fp );
    }

    initialize_timer( &total_timer );

    TreeNode root( NULL, -1, -1, LEFT_WALL, RIGHT_WALL, LEFT_WALL, RIGHT_WALL );
    QuadTree::Root = &root;
    FishThread::Root = &root;
    for ( int i = 0;i < n_fish;i++ ) {
        root.insert( &fish[i] );
    }

#pragma omp parallel default(shared) shared(threads,count) \
private(max_norm,dt,curr_time,steps,output_time,myID,moveTo,numThreads)

    {
        numThreads = omp_get_num_threads();
        myID = omp_get_thread_num();

#pragma omp master
        {
            start_timer( &total_timer );

            QuadTree::NumThreads = numThreads;
            QuadTree::NumFish = n_fish;
            FishThread::NumThreads = numThreads;
            threads = new FishThread[numThreads];
            //right now it's just 4 leaf nodes. splitUp
            root.splitUp( threads[0] );
            //a bunch of rootnodes got put into thread[0], remove them since it's not the real owner
            threads[0].clearRootsLeaves();

            int currentThread = 0, fishOwned = 0;
            root.distributeFish( currentThread, fishOwned, threads );
            for ( int i = 0; i < numThreads; ++i ) {
                threads[i].balance();
            }
        }
        //make sure everyone sees the new quadtree structure
#pragma omp barrier

        //clean up the rootNodes list
        threads[myID].cleanupRoots();
        //TODO remove below 2 statements
//        threads[myID].balance();
        //#pragma omp critical
        //
        //        {
        //            cout << "Thread " << myID << " started with " << threads[myID].nFishOwned << " fish" << endl;
        //        }

        for ( output_time = 0.0, curr_time = 0.0, steps = 0;
                curr_time <= end_time && steps < max_steps;
                curr_time += dt, ++steps ) {

            if ( outputp && curr_time >= output_time ) {
                //have one thread output the first line and set count
#pragma omp single
                {
                    count = 0;
                    //flush to write count
#pragma omp flush (count)

                    fprintf( output_fp, "%.5g (%.5g):\n", curr_time, dt );
                }
                //each thread take turns outputting its fish & flushing count
#pragma omp critical (outputFish)
                {
                    //flush to read count
#pragma omp flush (count)
                    threads[myID].outputFish( output_fp, curr_time, dt, count );
                    //flush to write count
#pragma omp flush (count)

                }
                output_time = curr_time + output_interval;
                //barrier to make sure everyone sees threads[] state
#pragma omp barrier
#pragma omp single

                {
                    /*
                     * sum up the cg's and find total cg
                     */
                    double cg_x = 0, cg_y = 0;
                    for ( int i = 0;i < numThreads;i++ ) {
                        cg_x += threads[i].cg_x;
                        cg_y += threads[i].cg_y;
                    }
                    cg_x /= n_fish;
                    cg_y /= n_fish;
                    fprintf( output_fp, "  cg: (%g, %g)\n", scale_coord( cg_x ),
                             scale_coord( cg_y ) );
                }
            }

            threads[myID].interactFish();

            threads[myID].computeNorm();

            /*
             * make sure that everyone computes the maxNorm then waits, so
             * that we can flush those maxNorm to view of memory
             * TODO see if just flushing maxNorm when it's written
             * and before it's read makes things faster
             * TODO does order of barrier and flush matter?
             * is one order faster?
             */
#pragma omp barrier
#pragma omp single copyprivate(max_norm)
            {
                max_norm = threads[0].maxNorm;
                for ( int i = 1;i < numThreads;i++ ) {
                    max_norm = f_max( threads[i].maxNorm, max_norm );
                }
            }

            dt = max_norm_change / max_norm;
            dt = f_max( dt, min_dt );
            dt = f_min( dt, max_dt );

//#pragma omp master
//            {
//                cout << "steps is " << steps << endl;
//            }

            threads[myID].moveFish( dt, myID );

            //now we have threads exchange fish that have moved
            for ( moveTo = ( myID + 1 ) % numThreads;
                    moveTo != myID;
                    moveTo = ( moveTo + 1 ) % numThreads ) {
#pragma omp barrier
                threads[myID].transferFish( moveTo );
            }
#pragma omp barrier
            threads[myID].balance();
#pragma omp barrier
        }

#pragma omp barrier
#pragma omp master
        {
            stop_timer( &total_timer );

            if ( outputp ) {
                output_fish( output_fp, curr_time, dt, fish, n_fish );
            }

            //            for ( int i = 0; i < numThreads; ++i )
            //            {
            //                threads[i].balance();
            //                cout << "Thread " << i << " ended with " << threads[i].nFishOwned << " fish" << endl;
            //            }
            //			printf("\tEnded at %g (%g), %d (%d) steps\n",
            //			       curr_time, end_time, steps, max_steps);

            //			printf("Number of OpenMP threads: %d\n"
            //			       "Total time taken: %g\n",
            //			       numThreads,
            //			       timer_duration(total_timer));
            cout
            << numThreads << "\t\t" << n_fish << "\t\t"
            << setprecision( 4 )  << timer_duration( total_timer ) << "\t\t"
            << curr_time << "\t" << steps << endl;
//            cout
//            << QuadTree::MinFish << "\t" << numCollapses << "\t\t"
//            << QuadTree::MaxFish << "\t" << numSplits << "\t\t"
//            << setprecision( 5 ) << timer_duration( total_timer ) << "\t\t"
//            << curr_time << "\t" << steps << endl;
            cout <<
            threads[myID].averageInteractions() << endl;
        }
    }
    delete []threads;
    free( fish );
    //TODO why does this fail?
    //	fclose(output_fp);
}

/* Allocate and initialize the fish positions / velocities / accelerations. */
void init_fish()
{
    int i, j;
    float xf, yf;
    fish = ( fish_t * ) malloc( n_fish * sizeof( fish_t ) );
    if ( !fish ) {
        perror( "malloc" );
        exit( -1 );
    }
    for ( i = 0; i < n_fish; ++i ) {
        if ( inputp ) {
            fscanf( input_fp, "  %d: (%f, %f)", &j, &xf, &yf );
            fish[i].x = unscale_coord( xf );
            fish[i].y = unscale_coord( yf );
        } else if ( uniformp ) {
            fish[i].x = unscale_coord( drand48() );
            fish[i].y = unscale_coord( drand48() );
        } else {
            const double angle = i * ( 2.0 * M_PI / n_fish );
            fish[i].x = unscale_coord( 0.5 * cos( angle ) + 0.5 );
            fish[i].y = unscale_coord( 0.5 * sin( angle ) + 0.5 );
        }
        fish[i].fishInteractedWith = 0;
        fish[i].numInteractions = 0;
    }
    for ( i = 0;i < n_fish;i++ ) {
        fish[i].ax = fish[i].ay = 0.0;
        if ( inputp ) {
            fscanf( input_fp, "  %d: (%f, %f)", &j, &xf, &yf );
            fish[i].vx = xf;
            fish[i].vy = yf;
        } else {
            fish[i].vx = START_CONST * ( drand48() - 0.5 );
            fish[i].vy = START_CONST * ( drand48() - 0.5 );
        }
    }
}

/*
  Dump out all the fishies (and their center of gravity)
  in a format that the viewer understands.
*/
void output_fish( FILE* output_fp, const double t, const double dt,
                  const fish_t* fish, const int n_fish )
{
    int i;
    double cg_x = 0.0;
    double cg_y = 0.0;

    fprintf( output_fp, "%.5g (%.5g):\n", t, dt );
    for ( i = 0; i < n_fish; ++i ) {
        cg_x += fish[i].x;
        cg_y += fish[i].y;
        fprintf( output_fp, "  %d: (%g, %g)\n", i,
                 scale_coord( fish[i].x ), scale_coord( fish[i].y ) );
    }
    if ( outvelp ) {
        for ( i = 0; i < n_fish; ++i ) {
            fprintf( output_fp, "  %d: (%g, %g)\n", i, fish[i].vx, fish[i].vy );
        }
    }
    cg_x /= n_fish;
    cg_y /= n_fish;
    fprintf( output_fp, "  cg: (%g, %g)\n", scale_coord( cg_x ),
             scale_coord( cg_y ) );
}

double distance2( double ax, double ay, double bx, double by )
{
    double dx = bx - ax;
    if ( fabs( dx ) > WALL_SEP - R_SIGHT ) {
        if ( dx < 0 ) {
            dx = fabs( dx ) - WALL_SEP;
        } else {
            dx = -fabs( dx ) + WALL_SEP;
        }
    }
    double dy = by - ay;
    if ( fabs( dy ) > WALL_SEP - R_SIGHT ) {
        if ( dy < 0 ) {
            dy = fabs( dy ) - WALL_SEP;
        } else {
            dy = -fabs( dy ) + WALL_SEP;
        }
    }
    double r_2 = dx * dx + dy * dy;
    return r_2;
}
